
_Soft_UART_Write_Text:

;tric-newsensors.c,32 :: 		void Soft_UART_Write_Text(char* testo) {
;tric-newsensors.c,34 :: 		for (cnt=0; cnt<strlen(testo); cnt++) {
	CLRF        Soft_UART_Write_Text_cnt_L0+0 
L_Soft_UART_Write_Text0:
	MOVF        FARG_Soft_UART_Write_Text_testo+0, 0 
	MOVWF       FARG_strlen_s+0 
	MOVF        FARG_Soft_UART_Write_Text_testo+1, 0 
	MOVWF       FARG_strlen_s+1 
	CALL        _strlen+0, 0
	MOVLW       128
	MOVWF       R2 
	MOVLW       128
	XORWF       R1, 0 
	SUBWF       R2, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__Soft_UART_Write_Text27
	MOVF        R0, 0 
	SUBWF       Soft_UART_Write_Text_cnt_L0+0, 0 
L__Soft_UART_Write_Text27:
	BTFSC       STATUS+0, 0 
	GOTO        L_Soft_UART_Write_Text1
;tric-newsensors.c,35 :: 		Soft_UART_Write(testo[cnt]);
	MOVF        Soft_UART_Write_Text_cnt_L0+0, 0 
	ADDWF       FARG_Soft_UART_Write_Text_testo+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_Soft_UART_Write_Text_testo+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_Soft_UART_Write_udata+0 
	CALL        _Soft_UART_Write+0, 0
;tric-newsensors.c,34 :: 		for (cnt=0; cnt<strlen(testo); cnt++) {
	INCF        Soft_UART_Write_Text_cnt_L0+0, 1 
;tric-newsensors.c,36 :: 		}
	GOTO        L_Soft_UART_Write_Text0
L_Soft_UART_Write_Text1:
;tric-newsensors.c,37 :: 		}
	RETURN      0
; end of _Soft_UART_Write_Text

_strstrip:

;tric-newsensors.c,39 :: 		char *strstrip(char *s)
;tric-newsensors.c,42 :: 		end = s + strlen(s) - 1;
	MOVF        FARG_strstrip_s+0, 0 
	MOVWF       FARG_strlen_s+0 
	MOVF        FARG_strstrip_s+1, 0 
	MOVWF       FARG_strlen_s+1 
	CALL        _strlen+0, 0
	MOVF        R0, 0 
	ADDWF       FARG_strstrip_s+0, 0 
	MOVWF       strstrip_end_L0+0 
	MOVF        R1, 0 
	ADDWFC      FARG_strstrip_s+1, 0 
	MOVWF       strstrip_end_L0+1 
	MOVLW       1
	SUBWF       strstrip_end_L0+0, 1 
	MOVLW       0
	SUBWFB      strstrip_end_L0+1, 1 
;tric-newsensors.c,43 :: 		while (end >= s && isspace(*end))
L_strstrip3:
	MOVF        FARG_strstrip_s+1, 0 
	SUBWF       strstrip_end_L0+1, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__strstrip28
	MOVF        FARG_strstrip_s+0, 0 
	SUBWF       strstrip_end_L0+0, 0 
L__strstrip28:
	BTFSS       STATUS+0, 0 
	GOTO        L_strstrip4
	MOVFF       strstrip_end_L0+0, FSR0L
	MOVFF       strstrip_end_L0+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_isspace_character+0 
	CALL        _isspace+0, 0
	MOVF        R0, 1 
	BTFSC       STATUS+0, 2 
	GOTO        L_strstrip4
L__strstrip25:
;tric-newsensors.c,44 :: 		end--;
	MOVLW       1
	SUBWF       strstrip_end_L0+0, 1 
	MOVLW       0
	SUBWFB      strstrip_end_L0+1, 1 
	GOTO        L_strstrip3
L_strstrip4:
;tric-newsensors.c,45 :: 		*(end + 1) = '\0';
	MOVLW       1
	ADDWF       strstrip_end_L0+0, 0 
	MOVWF       FSR1L 
	MOVLW       0
	ADDWFC      strstrip_end_L0+1, 0 
	MOVWF       FSR1H 
	CLRF        POSTINC1+0 
;tric-newsensors.c,47 :: 		while (*s && isspace(*s))
L_strstrip7:
	MOVFF       FARG_strstrip_s+0, FSR0L
	MOVFF       FARG_strstrip_s+1, FSR0H
	MOVF        POSTINC0+0, 1 
	BTFSC       STATUS+0, 2 
	GOTO        L_strstrip8
	MOVFF       FARG_strstrip_s+0, FSR0L
	MOVFF       FARG_strstrip_s+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_isspace_character+0 
	CALL        _isspace+0, 0
	MOVF        R0, 1 
	BTFSC       STATUS+0, 2 
	GOTO        L_strstrip8
L__strstrip24:
;tric-newsensors.c,48 :: 		s++;
	INFSNZ      FARG_strstrip_s+0, 1 
	INCF        FARG_strstrip_s+1, 1 
	GOTO        L_strstrip7
L_strstrip8:
;tric-newsensors.c,50 :: 		return s;
	MOVF        FARG_strstrip_s+0, 0 
	MOVWF       R0 
	MOVF        FARG_strstrip_s+1, 0 
	MOVWF       R1 
;tric-newsensors.c,51 :: 		}
	RETURN      0
; end of _strstrip

_PID_update:

;tric-newsensors.c,57 :: 		int PID_update() {
;tric-newsensors.c,65 :: 		cnt=0;
	CLRF        PID_update_cnt_L0+0 
;tric-newsensors.c,66 :: 		RX_values[0]=0;
	CLRF        _RX_values+0 
;tric-newsensors.c,69 :: 		PID_err_proportional[cnt] = RX_values[cnt] - roll;  // Need to be implemented conversion between RX and SENSORS values!!!
	CLRF        R0 
	CLRF        R1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       _RX_values+0
	MOVWF       FSR0L 
	MOVLW       hi_addr(_RX_values+0)
	MOVWF       FSR0H 
	MOVF        PID_update_cnt_L0+0, 0 
	ADDWF       FSR0L, 1 
	BTFSC       STATUS+0, 0 
	INCF        FSR0H, 1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	CALL        _Byte2Double+0, 0
	MOVF        _roll+0, 0 
	MOVWF       R4 
	MOVF        _roll+1, 0 
	MOVWF       R5 
	MOVF        _roll+2, 0 
	MOVWF       R6 
	MOVF        _roll+3, 0 
	MOVWF       R7 
	CALL        _Sub_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;tric-newsensors.c,70 :: 		if((PID_err_proportional[cnt] > PID_err_integral_min) || (PID_err_proportional[cnt] < -PID_err_integral_min)) PID_err_integral[cnt] += PID_err_proportional[cnt]* dt;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	MOVLW       0
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	MOVLW       0
	MOVWF       R2 
	MOVLW       128
	MOVWF       R3 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update26
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       128
	MOVWF       R6 
	MOVLW       128
	MOVWF       R7 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update26
	GOTO        L_PID_update13
L__PID_update26:
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_integral_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_integral_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR0L
	MOVFF       FLOC__PID_update+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R4 
	MOVF        POSTINC0+0, 0 
	MOVWF       R5 
	MOVF        POSTINC0+0, 0 
	MOVWF       R6 
	MOVF        POSTINC0+0, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
L_PID_update13:
;tric-newsensors.c,71 :: 		PID_err_integral[cnt] += PID_err_proportional[cnt] * dt;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_integral_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_integral_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR0L
	MOVFF       FLOC__PID_update+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R4 
	MOVF        POSTINC0+0, 0 
	MOVWF       R5 
	MOVF        POSTINC0+0, 0 
	MOVWF       R6 
	MOVF        POSTINC0+0, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;tric-newsensors.c,72 :: 		PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / dt;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_derivative_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_derivative_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R8 
	MOVF        POSTINC0+0, 0 
	MOVWF       R9 
	MOVF        POSTINC0+0, 0 
	MOVWF       R10 
	MOVF        POSTINC0+0, 0 
	MOVWF       R11 
	MOVLW       PID_update_PID_err_proportional_old_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_old_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R4 
	MOVF        POSTINC2+0, 0 
	MOVWF       R5 
	MOVF        POSTINC2+0, 0 
	MOVWF       R6 
	MOVF        POSTINC2+0, 0 
	MOVWF       R7 
	MOVF        R8, 0 
	MOVWF       R0 
	MOVF        R9, 0 
	MOVWF       R1 
	MOVF        R10, 0 
	MOVWF       R2 
	MOVF        R11, 0 
	MOVWF       R3 
	CALL        _Sub_32x32_FP+0, 0
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;tric-newsensors.c,73 :: 		PID_output[cnt] = (int)( (PID_Kp*PID_err_proportional[cnt]) + (PID_Ki*PID_err_integral[cnt]) + (PID_Kd*PID_err_derivative[cnt]) );
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+6 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+7 
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       FLOC__PID_update+4 
	MOVLW       0
	MOVWF       FLOC__PID_update+5 
	RLCF        FLOC__PID_update+4, 1 
	BCF         FLOC__PID_update+4, 0 
	RLCF        FLOC__PID_update+5, 1 
	RLCF        FLOC__PID_update+4, 1 
	BCF         FLOC__PID_update+4, 0 
	RLCF        FLOC__PID_update+5, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       FLOC__PID_update+4, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      FLOC__PID_update+5, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       51
	MOVWF       R4 
	MOVLW       51
	MOVWF       R5 
	MOVLW       51
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_update+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_update+3 
	MOVLW       PID_update_PID_err_integral_L0+0
	ADDWF       FLOC__PID_update+4, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_integral_L0+0)
	ADDWFC      FLOC__PID_update+5, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       205
	MOVWF       R4 
	MOVLW       204
	MOVWF       R5 
	MOVLW       76
	MOVWF       R6 
	MOVLW       124
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_update+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_update+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_update+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_update+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_update+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_update+3 
	MOVLW       PID_update_PID_err_derivative_L0+0
	ADDWF       FLOC__PID_update+4, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_derivative_L0+0)
	ADDWFC      FLOC__PID_update+5, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       10
	MOVWF       R4 
	MOVLW       215
	MOVWF       R5 
	MOVLW       35
	MOVWF       R6 
	MOVLW       120
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_update+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_update+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_update+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_update+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVFF       FLOC__PID_update+6, FSR1L
	MOVFF       FLOC__PID_update+7, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
;tric-newsensors.c,75 :: 		if(PID_output[cnt] > PID_output_max) PID_output[cnt] = PID_output_max;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVLW       128
	MOVWF       R0 
	MOVLW       128
	XORWF       R2, 0 
	SUBWF       R0, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update29
	MOVF        R1, 0 
	SUBLW       255
L__PID_update29:
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_update14
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	MOVLW       255
	MOVWF       POSTINC1+0 
	MOVLW       0
	MOVWF       POSTINC1+0 
	GOTO        L_PID_update15
L_PID_update14:
;tric-newsensors.c,76 :: 		else if(PID_output[cnt] < PID_output_min) PID_output[cnt] = PID_output_min;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVLW       128
	XORWF       R2, 0 
	MOVWF       R0 
	MOVLW       128
	SUBWF       R0, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update30
	MOVLW       0
	SUBWF       R1, 0 
L__PID_update30:
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_update16
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	CLRF        POSTINC1+0 
	CLRF        POSTINC1+0 
L_PID_update16:
L_PID_update15:
;tric-newsensors.c,78 :: 		PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_old_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_old_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
;tric-newsensors.c,80 :: 		return PID_output[0];
	MOVF        PID_update_PID_output_L0+0, 0 
	MOVWF       R0 
	MOVF        PID_update_PID_output_L0+1, 0 
	MOVWF       R1 
;tric-newsensors.c,81 :: 		}
	RETURN      0
; end of _PID_update

_Complementary:

;tric-newsensors.c,83 :: 		float Complementary(float oldAngle, float newAngle, float newRate) {
;tric-newsensors.c,84 :: 		aa=tau/(tau+dt);
	MOVF        _tau+0, 0 
	MOVWF       R0 
	MOVF        _tau+1, 0 
	MOVWF       R1 
	MOVF        _tau+2, 0 
	MOVWF       R2 
	MOVF        _tau+3, 0 
	MOVWF       R3 
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	MOVF        _tau+0, 0 
	MOVWF       R0 
	MOVF        _tau+1, 0 
	MOVWF       R1 
	MOVF        _tau+2, 0 
	MOVWF       R2 
	MOVF        _tau+3, 0 
	MOVWF       R3 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       _aa+0 
	MOVF        R1, 0 
	MOVWF       _aa+1 
	MOVF        R2, 0 
	MOVWF       _aa+2 
	MOVF        R3, 0 
	MOVWF       _aa+3 
;tric-newsensors.c,86 :: 		oldAngle= aa* (oldAngle + newRate * dt) + (1-aa) * (newAngle);
	MOVF        FARG_Complementary_newRate+0, 0 
	MOVWF       R0 
	MOVF        FARG_Complementary_newRate+1, 0 
	MOVWF       R1 
	MOVF        FARG_Complementary_newRate+2, 0 
	MOVWF       R2 
	MOVF        FARG_Complementary_newRate+3, 0 
	MOVWF       R3 
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FARG_Complementary_oldAngle+0, 0 
	MOVWF       R4 
	MOVF        FARG_Complementary_oldAngle+1, 0 
	MOVWF       R5 
	MOVF        FARG_Complementary_oldAngle+2, 0 
	MOVWF       R6 
	MOVF        FARG_Complementary_oldAngle+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        _aa+0, 0 
	MOVWF       R4 
	MOVF        _aa+1, 0 
	MOVWF       R5 
	MOVF        _aa+2, 0 
	MOVWF       R6 
	MOVF        _aa+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__Complementary+0 
	MOVF        R1, 0 
	MOVWF       FLOC__Complementary+1 
	MOVF        R2, 0 
	MOVWF       FLOC__Complementary+2 
	MOVF        R3, 0 
	MOVWF       FLOC__Complementary+3 
	MOVF        _aa+0, 0 
	MOVWF       R4 
	MOVF        _aa+1, 0 
	MOVWF       R5 
	MOVF        _aa+2, 0 
	MOVWF       R6 
	MOVF        _aa+3, 0 
	MOVWF       R7 
	MOVLW       0
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	MOVLW       0
	MOVWF       R2 
	MOVLW       127
	MOVWF       R3 
	CALL        _Sub_32x32_FP+0, 0
	MOVF        FARG_Complementary_newAngle+0, 0 
	MOVWF       R4 
	MOVF        FARG_Complementary_newAngle+1, 0 
	MOVWF       R5 
	MOVF        FARG_Complementary_newAngle+2, 0 
	MOVWF       R6 
	MOVF        FARG_Complementary_newAngle+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__Complementary+0, 0 
	MOVWF       R4 
	MOVF        FLOC__Complementary+1, 0 
	MOVWF       R5 
	MOVF        FLOC__Complementary+2, 0 
	MOVWF       R6 
	MOVF        FLOC__Complementary+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_Complementary_oldAngle+0 
	MOVF        R1, 0 
	MOVWF       FARG_Complementary_oldAngle+1 
	MOVF        R2, 0 
	MOVWF       FARG_Complementary_oldAngle+2 
	MOVF        R3, 0 
	MOVWF       FARG_Complementary_oldAngle+3 
;tric-newsensors.c,87 :: 		return oldAngle;
;tric-newsensors.c,88 :: 		}
	RETURN      0
; end of _Complementary

_main:

;tric-newsensors.c,90 :: 		void main() {
;tric-newsensors.c,96 :: 		LED_DIR = 0;      // LED is set as OUTPUT
	BCF         TRISC+0, 2 
;tric-newsensors.c,98 :: 		ADCON1 = 0x0F;    // Set A/D Disabled
	MOVLW       15
	MOVWF       ADCON1+0 
;tric-newsensors.c,99 :: 		CMCON = 0xCF;     // Set Comparator Disabled
	MOVLW       207
	MOVWF       CMCON+0 
;tric-newsensors.c,101 :: 		Soft_UART_Init(&PORTB, 2, 3, 115200, 0);
	MOVLW       PORTB+0
	MOVWF       FARG_Soft_UART_Init_port+0 
	MOVLW       hi_addr(PORTB+0)
	MOVWF       FARG_Soft_UART_Init_port+1 
	MOVLW       2
	MOVWF       FARG_Soft_UART_Init_rx_pin+0 
	MOVLW       3
	MOVWF       FARG_Soft_UART_Init_tx_pin+0 
	MOVLW       0
	MOVWF       FARG_Soft_UART_Init_baud_rate+0 
	MOVLW       194
	MOVWF       FARG_Soft_UART_Init_baud_rate+1 
	MOVLW       1
	MOVWF       FARG_Soft_UART_Init_baud_rate+2 
	MOVLW       0
	MOVWF       FARG_Soft_UART_Init_baud_rate+3 
	CLRF        FARG_Soft_UART_Init_inverted+0 
	CALL        _Soft_UART_Init+0, 0
;tric-newsensors.c,103 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_main17:
	DECFSZ      R13, 1, 1
	BRA         L_main17
	DECFSZ      R12, 1, 1
	BRA         L_main17
	DECFSZ      R11, 1, 1
	BRA         L_main17
	NOP
;tric-newsensors.c,104 :: 		I2C1_Init(400000);
	MOVLW       30
	MOVWF       SSPADD+0 
	CALL        _I2C1_Init+0, 0
;tric-newsensors.c,105 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_main18:
	DECFSZ      R13, 1, 1
	BRA         L_main18
	DECFSZ      R12, 1, 1
	BRA         L_main18
	DECFSZ      R11, 1, 1
	BRA         L_main18
	NOP
;tric-newsensors.c,106 :: 		LED=1;
	BSF         PORTC+0, 2 
;tric-newsensors.c,108 :: 		BMA180_init();
	CALL        _BMA180_init+0, 0
;tric-newsensors.c,109 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_main19:
	DECFSZ      R13, 1, 1
	BRA         L_main19
	DECFSZ      R12, 1, 1
	BRA         L_main19
	DECFSZ      R11, 1, 1
	BRA         L_main19
	NOP
;tric-newsensors.c,110 :: 		ITG3200_init();
	CALL        _ITG3200_init+0, 0
;tric-newsensors.c,111 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_main20:
	DECFSZ      R13, 1, 1
	BRA         L_main20
	DECFSZ      R12, 1, 1
	BRA         L_main20
	DECFSZ      R11, 1, 1
	BRA         L_main20
	NOP
;tric-newsensors.c,112 :: 		LED=0;
	BCF         PORTC+0, 2 
;tric-newsensors.c,114 :: 		while (1) {
L_main21:
;tric-newsensors.c,115 :: 		LED = !LED;
	BTG         PORTC+0, 2 
;tric-newsensors.c,117 :: 		BMA180_readAcc();
	CALL        _BMA180_readAcc+0, 0
;tric-newsensors.c,118 :: 		ITG3200_readGyro();
	CALL        _ITG3200_readGyro+0, 0
;tric-newsensors.c,120 :: 		gyro[0] = gyro[0] / 14.375;
	MOVF        _gyro+0, 0 
	MOVWF       R0 
	MOVF        _gyro+1, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       102
	MOVWF       R6 
	MOVLW       130
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       _gyro+0 
	MOVF        R1, 0 
	MOVWF       _gyro+1 
;tric-newsensors.c,121 :: 		gyro[1] = gyro[1] / 14.375;
	MOVF        _gyro+2, 0 
	MOVWF       R0 
	MOVF        _gyro+3, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       102
	MOVWF       R6 
	MOVLW       130
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       _gyro+2 
	MOVF        R1, 0 
	MOVWF       _gyro+3 
;tric-newsensors.c,122 :: 		gyro[2] = gyro[2] / 14.375;
	MOVF        _gyro+4, 0 
	MOVWF       R0 
	MOVF        _gyro+5, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       102
	MOVWF       R6 
	MOVLW       130
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       _gyro+4 
	MOVF        R1, 0 
	MOVWF       _gyro+5 
;tric-newsensors.c,124 :: 		accf[0] = acc[0] / 4096.0;
	MOVF        _acc+0, 0 
	MOVWF       R0 
	MOVF        _acc+1, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       139
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       _accf+0 
	MOVF        R1, 0 
	MOVWF       _accf+1 
	MOVF        R2, 0 
	MOVWF       _accf+2 
	MOVF        R3, 0 
	MOVWF       _accf+3 
;tric-newsensors.c,125 :: 		accf[1] = acc[1] / 4096.0;
	MOVF        _acc+2, 0 
	MOVWF       R0 
	MOVF        _acc+3, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       139
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       _accf+4 
	MOVF        R1, 0 
	MOVWF       _accf+5 
	MOVF        R2, 0 
	MOVWF       _accf+6 
	MOVF        R3, 0 
	MOVWF       _accf+7 
;tric-newsensors.c,126 :: 		accf[2] = acc[2] / 4096.0;
	MOVF        _acc+4, 0 
	MOVWF       R0 
	MOVF        _acc+5, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       139
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       _accf+8 
	MOVF        R1, 0 
	MOVWF       _accf+9 
	MOVF        R2, 0 
	MOVWF       _accf+10 
	MOVF        R3, 0 
	MOVWF       _accf+11 
;tric-newsensors.c,128 :: 		modulo = sqrt(accf[0]*accf[0]+accf[1]*accf[1]+accf[2]*accf[2]);
	MOVF        _accf+0, 0 
	MOVWF       R0 
	MOVF        _accf+1, 0 
	MOVWF       R1 
	MOVF        _accf+2, 0 
	MOVWF       R2 
	MOVF        _accf+3, 0 
	MOVWF       R3 
	MOVF        _accf+0, 0 
	MOVWF       R4 
	MOVF        _accf+1, 0 
	MOVWF       R5 
	MOVF        _accf+2, 0 
	MOVWF       R6 
	MOVF        _accf+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__main+0 
	MOVF        R1, 0 
	MOVWF       FLOC__main+1 
	MOVF        R2, 0 
	MOVWF       FLOC__main+2 
	MOVF        R3, 0 
	MOVWF       FLOC__main+3 
	MOVF        _accf+4, 0 
	MOVWF       R0 
	MOVF        _accf+5, 0 
	MOVWF       R1 
	MOVF        _accf+6, 0 
	MOVWF       R2 
	MOVF        _accf+7, 0 
	MOVWF       R3 
	MOVF        _accf+4, 0 
	MOVWF       R4 
	MOVF        _accf+5, 0 
	MOVWF       R5 
	MOVF        _accf+6, 0 
	MOVWF       R6 
	MOVF        _accf+7, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__main+0, 0 
	MOVWF       R4 
	MOVF        FLOC__main+1, 0 
	MOVWF       R5 
	MOVF        FLOC__main+2, 0 
	MOVWF       R6 
	MOVF        FLOC__main+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__main+0 
	MOVF        R1, 0 
	MOVWF       FLOC__main+1 
	MOVF        R2, 0 
	MOVWF       FLOC__main+2 
	MOVF        R3, 0 
	MOVWF       FLOC__main+3 
	MOVF        _accf+8, 0 
	MOVWF       R0 
	MOVF        _accf+9, 0 
	MOVWF       R1 
	MOVF        _accf+10, 0 
	MOVWF       R2 
	MOVF        _accf+11, 0 
	MOVWF       R3 
	MOVF        _accf+8, 0 
	MOVWF       R4 
	MOVF        _accf+9, 0 
	MOVWF       R5 
	MOVF        _accf+10, 0 
	MOVWF       R6 
	MOVF        _accf+11, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__main+0, 0 
	MOVWF       R4 
	MOVF        FLOC__main+1, 0 
	MOVWF       R5 
	MOVF        FLOC__main+2, 0 
	MOVWF       R6 
	MOVF        FLOC__main+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_sqrt_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_sqrt_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_sqrt_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_sqrt_x+3 
	CALL        _sqrt+0, 0
	MOVF        R0, 0 
	MOVWF       _modulo+0 
	MOVF        R1, 0 
	MOVWF       _modulo+1 
	MOVF        R2, 0 
	MOVWF       _modulo+2 
	MOVF        R3, 0 
	MOVWF       _modulo+3 
;tric-newsensors.c,129 :: 		roll = Complementary(roll, asin(accf[1]/modulo)*57.295779, -gyro[0]);
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	MOVF        _accf+4, 0 
	MOVWF       R0 
	MOVF        _accf+5, 0 
	MOVWF       R1 
	MOVF        _accf+6, 0 
	MOVWF       R2 
	MOVF        _accf+7, 0 
	MOVWF       R3 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_asin_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_asin_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_asin_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_asin_x+3 
	CALL        _asin+0, 0
	MOVLW       225
	MOVWF       R4 
	MOVLW       46
	MOVWF       R5 
	MOVLW       101
	MOVWF       R6 
	MOVLW       132
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_Complementary_newAngle+0 
	MOVF        R1, 0 
	MOVWF       FARG_Complementary_newAngle+1 
	MOVF        R2, 0 
	MOVWF       FARG_Complementary_newAngle+2 
	MOVF        R3, 0 
	MOVWF       FARG_Complementary_newAngle+3 
	MOVF        _roll+0, 0 
	MOVWF       FARG_Complementary_oldAngle+0 
	MOVF        _roll+1, 0 
	MOVWF       FARG_Complementary_oldAngle+1 
	MOVF        _roll+2, 0 
	MOVWF       FARG_Complementary_oldAngle+2 
	MOVF        _roll+3, 0 
	MOVWF       FARG_Complementary_oldAngle+3 
	MOVF        _gyro+0, 0 
	SUBLW       0
	MOVWF       R0 
	MOVF        _gyro+1, 0 
	MOVWF       R1 
	MOVLW       0
	SUBFWB      R1, 1 
	CALL        _Int2Double+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_Complementary_newRate+0 
	MOVF        R1, 0 
	MOVWF       FARG_Complementary_newRate+1 
	MOVF        R2, 0 
	MOVWF       FARG_Complementary_newRate+2 
	MOVF        R3, 0 
	MOVWF       FARG_Complementary_newRate+3 
	CALL        _Complementary+0, 0
	MOVF        R0, 0 
	MOVWF       _roll+0 
	MOVF        R1, 0 
	MOVWF       _roll+1 
	MOVF        R2, 0 
	MOVWF       _roll+2 
	MOVF        R3, 0 
	MOVWF       _roll+3 
;tric-newsensors.c,130 :: 		pitch = Complementary(pitch, asin(accf[0]/modulo)*57.295779, gyro[1]);
	MOVF        _modulo+0, 0 
	MOVWF       R4 
	MOVF        _modulo+1, 0 
	MOVWF       R5 
	MOVF        _modulo+2, 0 
	MOVWF       R6 
	MOVF        _modulo+3, 0 
	MOVWF       R7 
	MOVF        _accf+0, 0 
	MOVWF       R0 
	MOVF        _accf+1, 0 
	MOVWF       R1 
	MOVF        _accf+2, 0 
	MOVWF       R2 
	MOVF        _accf+3, 0 
	MOVWF       R3 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_asin_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_asin_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_asin_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_asin_x+3 
	CALL        _asin+0, 0
	MOVLW       225
	MOVWF       R4 
	MOVLW       46
	MOVWF       R5 
	MOVLW       101
	MOVWF       R6 
	MOVLW       132
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_Complementary_newAngle+0 
	MOVF        R1, 0 
	MOVWF       FARG_Complementary_newAngle+1 
	MOVF        R2, 0 
	MOVWF       FARG_Complementary_newAngle+2 
	MOVF        R3, 0 
	MOVWF       FARG_Complementary_newAngle+3 
	MOVF        _pitch+0, 0 
	MOVWF       FARG_Complementary_oldAngle+0 
	MOVF        _pitch+1, 0 
	MOVWF       FARG_Complementary_oldAngle+1 
	MOVF        _pitch+2, 0 
	MOVWF       FARG_Complementary_oldAngle+2 
	MOVF        _pitch+3, 0 
	MOVWF       FARG_Complementary_oldAngle+3 
	MOVF        _gyro+2, 0 
	MOVWF       R0 
	MOVF        _gyro+3, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_Complementary_newRate+0 
	MOVF        R1, 0 
	MOVWF       FARG_Complementary_newRate+1 
	MOVF        R2, 0 
	MOVWF       FARG_Complementary_newRate+2 
	MOVF        R3, 0 
	MOVWF       FARG_Complementary_newRate+3 
	CALL        _Complementary+0, 0
	MOVF        R0, 0 
	MOVWF       _pitch+0 
	MOVF        R1, 0 
	MOVWF       _pitch+1 
	MOVF        R2, 0 
	MOVWF       _pitch+2 
	MOVF        R3, 0 
	MOVWF       _pitch+3 
;tric-newsensors.c,131 :: 		yaw += (gyro[2]*dt);
	MOVF        _gyro+4, 0 
	MOVWF       R0 
	MOVF        _gyro+5, 0 
	MOVWF       R1 
	CALL        _Int2Double+0, 0
	MOVF        _dt+0, 0 
	MOVWF       R4 
	MOVF        _dt+1, 0 
	MOVWF       R5 
	MOVF        _dt+2, 0 
	MOVWF       R6 
	MOVF        _dt+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        _yaw+0, 0 
	MOVWF       R4 
	MOVF        _yaw+1, 0 
	MOVWF       R5 
	MOVF        _yaw+2, 0 
	MOVWF       R6 
	MOVF        _yaw+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       _yaw+0 
	MOVF        R1, 0 
	MOVWF       _yaw+1 
	MOVF        R2, 0 
	MOVWF       _yaw+2 
	MOVF        R3, 0 
	MOVWF       _yaw+3 
;tric-newsensors.c,133 :: 		RX_values[0] = 0;
	CLRF        _RX_values+0 
;tric-newsensors.c,135 :: 		IntToStr(PID_update(), v_string);
	CALL        _PID_update+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_IntToStr_input+0 
	MOVF        R1, 0 
	MOVWF       FARG_IntToStr_input+1 
	MOVLW       main_v_string_L0+0
	MOVWF       FARG_IntToStr_output+0 
	MOVLW       hi_addr(main_v_string_L0+0)
	MOVWF       FARG_IntToStr_output+1 
	CALL        _IntToStr+0, 0
;tric-newsensors.c,136 :: 		IntToStr(floor(roll), x_string);
	MOVF        _roll+0, 0 
	MOVWF       FARG_floor_x+0 
	MOVF        _roll+1, 0 
	MOVWF       FARG_floor_x+1 
	MOVF        _roll+2, 0 
	MOVWF       FARG_floor_x+2 
	MOVF        _roll+3, 0 
	MOVWF       FARG_floor_x+3 
	CALL        _floor+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_IntToStr_input+0 
	MOVF        R1, 0 
	MOVWF       FARG_IntToStr_input+1 
	MOVLW       main_x_string_L0+0
	MOVWF       FARG_IntToStr_output+0 
	MOVLW       hi_addr(main_x_string_L0+0)
	MOVWF       FARG_IntToStr_output+1 
	CALL        _IntToStr+0, 0
;tric-newsensors.c,137 :: 		IntToStr(floor(asin(accf[1]/modulo)*57.295779), y_string);
	MOVF        _modulo+0, 0 
	MOVWF       R4 
	MOVF        _modulo+1, 0 
	MOVWF       R5 
	MOVF        _modulo+2, 0 
	MOVWF       R6 
	MOVF        _modulo+3, 0 
	MOVWF       R7 
	MOVF        _accf+4, 0 
	MOVWF       R0 
	MOVF        _accf+5, 0 
	MOVWF       R1 
	MOVF        _accf+6, 0 
	MOVWF       R2 
	MOVF        _accf+7, 0 
	MOVWF       R3 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_asin_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_asin_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_asin_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_asin_x+3 
	CALL        _asin+0, 0
	MOVLW       225
	MOVWF       R4 
	MOVLW       46
	MOVWF       R5 
	MOVLW       101
	MOVWF       R6 
	MOVLW       132
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_floor_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_floor_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_floor_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_floor_x+3 
	CALL        _floor+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_IntToStr_input+0 
	MOVF        R1, 0 
	MOVWF       FARG_IntToStr_input+1 
	MOVLW       main_y_string_L0+0
	MOVWF       FARG_IntToStr_output+0 
	MOVLW       hi_addr(main_y_string_L0+0)
	MOVWF       FARG_IntToStr_output+1 
	CALL        _IntToStr+0, 0
;tric-newsensors.c,138 :: 		IntToStr(floor(yaw), z_string);
	MOVF        _yaw+0, 0 
	MOVWF       FARG_floor_x+0 
	MOVF        _yaw+1, 0 
	MOVWF       FARG_floor_x+1 
	MOVF        _yaw+2, 0 
	MOVWF       FARG_floor_x+2 
	MOVF        _yaw+3, 0 
	MOVWF       FARG_floor_x+3 
	CALL        _floor+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_IntToStr_input+0 
	MOVF        R1, 0 
	MOVWF       FARG_IntToStr_input+1 
	MOVLW       main_z_string_L0+0
	MOVWF       FARG_IntToStr_output+0 
	MOVLW       hi_addr(main_z_string_L0+0)
	MOVWF       FARG_IntToStr_output+1 
	CALL        _IntToStr+0, 0
;tric-newsensors.c,141 :: 		Soft_UART_Write_Text("100,");
	MOVLW       ?lstr1_tric_45newsensors+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(?lstr1_tric_45newsensors+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,142 :: 		Soft_UART_Write_Text(&x_string);
	MOVLW       main_x_string_L0+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(main_x_string_L0+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,143 :: 		Soft_UART_Write_Text(",");
	MOVLW       ?lstr2_tric_45newsensors+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(?lstr2_tric_45newsensors+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,144 :: 		Soft_UART_Write_Text(&y_string);
	MOVLW       main_y_string_L0+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(main_y_string_L0+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,145 :: 		Soft_UART_Write_Text(",");
	MOVLW       ?lstr3_tric_45newsensors+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(?lstr3_tric_45newsensors+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,146 :: 		Soft_UART_Write_Text(&z_string);
	MOVLW       main_z_string_L0+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(main_z_string_L0+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,147 :: 		Soft_UART_Write_Text(",");
	MOVLW       ?lstr4_tric_45newsensors+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(?lstr4_tric_45newsensors+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,148 :: 		Soft_UART_Write_Text(&v_string);
	MOVLW       main_v_string_L0+0
	MOVWF       FARG_Soft_UART_Write_Text_testo+0 
	MOVLW       hi_addr(main_v_string_L0+0)
	MOVWF       FARG_Soft_UART_Write_Text_testo+1 
	CALL        _Soft_UART_Write_Text+0, 0
;tric-newsensors.c,150 :: 		Soft_UART_Write(10);
	MOVLW       10
	MOVWF       FARG_Soft_UART_Write_udata+0 
	CALL        _Soft_UART_Write+0, 0
;tric-newsensors.c,165 :: 		Delay_ms(20);
	MOVLW       2
	MOVWF       R11, 0
	MOVLW       56
	MOVWF       R12, 0
	MOVLW       173
	MOVWF       R13, 0
L_main23:
	DECFSZ      R13, 1, 1
	BRA         L_main23
	DECFSZ      R12, 1, 1
	BRA         L_main23
	DECFSZ      R11, 1, 1
	BRA         L_main23
;tric-newsensors.c,166 :: 		}
	GOTO        L_main21
;tric-newsensors.c,167 :: 		}
	GOTO        $+0
; end of _main
